#pragma once
#include <ros/ros.h>
#include <iostream>
#include <queue>
#include <unordered_map>
#include <yaml-cpp/yaml.h>

#include "trajectory/trajectory_custom_class.h"

#include "common_lib/common.h"
#include "common_lib/log.h"
#include "common_lib/transform.h"

#include "amr_trajectory_msgs/amr_trajectory_msg.h"
#include "amr_trajectory_msgs/path_info.h"
#include "task_msgs/task_msg.h"

#include "common_lib/topo_map.h"
#include "common_lib/speed_plan.h"

namespace trajectory
{
    struct SpeedPlanRecord
    {
        uint32_t end;
        float max_speed;
        std::string move_direction;
    };
    class Trajectory
    {
    public:
        Trajectory(ros::NodeHandle &nh, ros::NodeHandle &private_nh);
        ~Trajectory();

        void Init();
        void Start();
    private:
        ros::NodeHandle nh_;
        ros::NodeHandle private_nh_;

        std::string map_file_;
        std::string param_file_;
        std::string file_path_;

        ros::Subscriber sub_task_;
        ros::Subscriber sub_localization_;
        ros::Publisher  pub_trajectory_;

        float step_size_ = 0.02f;
        float point_id_dis_precision_ = 0.1f;
        float amr_acc_ = 0.0f;
        float amr_dcc_ = 0.0f;

        std::queue<trajectory::TaskClass> task_queue_;
        amr_trajectory_msgs::amr_trajectory_msg task_trajectory_;

        common_lib::Common com_;
        common_lib::TrapezoidalSpeedPlan trap_;
        std::unordered_map<uint32_t, SpeedPlanRecord> um_record_;

        void TaskCallback(const task_msgs::task_msgConstPtr &msg);
        void TrajectoryProcess();
        void MoveTaskProcess();
        void WorkTaskProcess();

        amr_trajectory_msgs::amr_trajectory_msg LinePro(task_msgs::task_path &path,    std::string id);
        amr_trajectory_msgs::amr_trajectory_msg CirclePro(task_msgs::task_path &path,  std::string id);
        amr_trajectory_msgs::amr_trajectory_msg BsplinePro(task_msgs::task_path &path, std::string id);

        void CalcHeading(amr_trajectory_msgs::amr_trajectory_msg &trajectory);
        void CalcDistance(amr_trajectory_msgs::amr_trajectory_msg &trajectory);
        void SpeedPlan(amr_trajectory_msgs::amr_trajectory_msg &trajectory, float acc, float dcc);

        typedef amr_trajectory_msgs::amr_trajectory_msg (Trajectory::*Fun)(task_msgs::task_path&, std::string );
        std::unordered_map<std::string, Fun> um_path_ = {{"line", &Trajectory::LinePro}, {"circle", &Trajectory::CirclePro}, {"bspline", &Trajectory::BsplinePro}};
        bool RepeatDetection(geometry_msgs::Pose new_point, amr_trajectory_msgs::amr_trajectory_msg &trajectory);

        void GenerateTaskMsg(void);
    };
}